//
// Created by yu on 2023/11/8.
//
#include <vector>
#include <iostream>
#include <iomanip>
#include <ros/ros.h>

#include "mi-motor.h"
#include "CANFrame.h"
#include "RobotMath.h"

using namespace std;

void miMotor::motorRun(uint8_t id)
{
    uint32_t runOrder = CAN::CreateCanExtendedId(id, 0, 3);
    //// data 可以为任意值
    vector<string> data = {"7f", "ff", "7f", "ff", "00", "00", "00", "00"};
    vector<unsigned char> sendData = CAN::CreateCanExtendedFrame(runOrder, data);
    CANserialPort.write(sendData);
}

void miMotor::motorStop(uint8_t id)
{
    uint32_t stopOrder = CAN::CreateCanExtendedId(id, 0, 4);
    //// data 可以为任意值
    vector<string> data = {"7f", "ff", "7f", "ff", "00", "00", "00", "00"};
    vector<unsigned char> sendData = CAN::CreateCanExtendedFrame(stopOrder, data);
    CANserialPort.write(sendData);
}

void miMotor::runControl(uint8_t id, float torque, uint16_t angle, float angularVelocity)
{
    ///torque           单位 Nm
    ///angle            单位 dsp
    ///angularVelocity  单位 degree/s
    uint16_t torque16, angle16, angularVelocity16;

//  数据单位转换成uint16_t
    torque16 = uint16_t(torque * 2730.667) + 32768;
    angularVelocity16 = uint16_t(angularVelocity * 19.063) + 32768;

    miMotor::motorRun(id);
    string angle1, angle2, angleV1, angleV2;
    stringstream ss1, ss2, ss3, ss4;
    uint8_t num;

    num = (angle >> 8) & 0xFF;
    ss1 << hex << setw(2) << setfill('0') << static_cast<unsigned>(num);
    angle1 = ss1.str();

    num = angle & 0xFF;
    ss2 << hex << setw(2) << setfill('0') << static_cast<unsigned>(num);
    angle2 = ss2.str();

    num = (angularVelocity16 >> 8) & 0xFF;
    ss3 << hex << setw(2) << setfill('0') << static_cast<unsigned>(num);
    angleV1 = ss3.str();

    num = angularVelocity16 & 0xFF;
    ss4 << hex << setw(2) << setfill('0') << static_cast<unsigned>(num);
    angleV2 = ss4.str();

    vector<string> data = {angle1, angle2, angleV1, angleV2, "00", "00", "00", "00"};

    uint32_t Order = CAN::CreateCanExtendedId(id, torque16, 1);

    vector<unsigned char> sendData = CAN::CreateCanExtendedFrame(Order, data);

    CANserialPort.write(sendData);

/// 读反馈数据
    string returnData = CANserialPort.read(CANserialPort.available());
    CAN::CANFrame frame{};
    frame = CAN::ReadExtendedFrame(returnData);
    uint16_t angleHigh, angleLow, torqueHigh, torqueLow, angularVHigh, angularVLow;
    miMotor::miReturnData motorData{};
    int sum = 0;
    for (const auto &da: frame.data)
    {
        sum = sum + da;
    }

    if (sum != 0)
    {
        motorData.id = id;

        angleHigh = frame.data[0] << 8;
        angleLow = frame.data[1];
        int angleCal = angleHigh | angleLow;
        motorData.angle = angleCal;

        angularVHigh = frame.data[2] << 8;
        angularVLow = frame.data[3];
        int angularVCal = angularVHigh | angularVLow;
        motorData.angularVelocity = (angularVCal - 32768) / 19.063;

        torqueHigh = frame.data[4] << 8;
        torqueLow = frame.data[5];
        int torqueCal = torqueHigh | torqueLow;
        motorData.torque = (torqueCal - 32768) / 2730.667;
        motorData = math.LowPassFilter(0.9, motorData);
    }

    else
    {
        if (frame.id == 1)
            motorData = Motor1;
        else
            motorData = Motor2;
    }
    if (frame.id == 1)
        Motor1 = motorData;
    else
        Motor2 = motorData;
}

void miMotor::positionControl(uint8_t id, float speedLim, int position)
{
    uint32_t speed16 = uint32_t((speedLim * 19.063) + 32768);

    vector<unsigned char> runmode{}, lim{}, pos{};
    stringstream ss1, ss2, ss3, ss4, ss5, ss6, ss7, ss8;

    uint32_t extendedId = CAN::CreateCanExtendedId(id, 0, 18);

//  设置runmode
    vector<string> runmodeData = {"70", "05", "00", "00", "00", "00", "00", "01"};
    runmode = CAN::CreateCanExtendedFrame(extendedId, runmodeData);
    CANserialPort.write(runmode);
//  运行开始
    motorRun(id);

//  设置速度限制
    uint8_t limHigh1 = (speed16 >> 24) & 0xFF;
    ss1 << hex << setw(2) << setfill('0') << static_cast<unsigned>(limHigh1);
    string lim1 = ss1.str();

    uint8_t limHigh2 = (speed16 >> 16) & 0xFF;
    ss2 << hex << setw(2) << setfill('0') << static_cast<unsigned>(limHigh2);
    string lim2 = ss2.str();

    uint8_t limLow1 = (speed16 >> 8) & 0xFF;
    ss3 << hex << setw(2) << setfill('0') << static_cast<unsigned>(limLow1);
    string lim3 = ss3.str();

    uint8_t limLow2 = speed16 & 0xFF;
    ss4 << hex << setw(2) << setfill('0') << static_cast<unsigned>(limLow2);
    string lim4 = ss4.str();

    vector<string> limData = {"70", "17", "00", "00", lim1, lim2, lim3, lim4};
    lim = CAN::CreateCanExtendedFrame(extendedId, limData);
    CANserialPort.write(lim);
    string a1 = CANserialPort.read(CANserialPort.available());
    string a2 = CAN::SerialDecode(a1);
    cout << a2 << endl;
//    ROS_INFO("%s",a1.c_str());
//  设置位置
    position = uint32_t(position);
    uint8_t posHigh1 = (position >> 24) & 0xFF;
    ss5 << hex << setw(2) << setfill('0') << static_cast<unsigned>(posHigh1);
    string pos1 = ss5.str();

    uint8_t posHigh2 = (position >> 16) & 0xFF;
    ss6 << hex << setw(2) << setfill('0') << static_cast<unsigned>(posHigh2);
    string pos2 = ss6.str();

    uint8_t posLow1 = (position >> 8) & 0xFF;
    ss7 << hex << setw(2) << setfill('0') << static_cast<unsigned>(posLow1);
    string pos3 = ss7.str();

    uint8_t posLow2 = position & 0xFF;
    ss8 << hex << setw(2) << setfill('0') << static_cast<unsigned>(posLow2);
    string pos4 = ss8.str();

    vector<string> posData = {"70", "16", "00", "00", pos1, pos2, pos3, pos4};
    pos = CAN::CreateCanExtendedFrame(extendedId, posData);
    CANserialPort.write(pos);
}

int miMotor::readAngle(uint8_t id)
{
    miMotor::motorRun(id);
    string returnData = CANserialPort.read(CANserialPort.available());
    CAN::CANFrame frame{};

    uint16_t angle;
    int sum = 0;
    for (auto ch: frame.data)
    {
        cout << ch << endl;
    }
    for (const auto &da: frame.data)
    {
        sum = sum + da;
    }

    if (sum != 0 && frame.id == id)
    {
        frame = CAN::ReadExtendedFrame(returnData);
        uint16_t angleHigh, angleLow;
        angleHigh = frame.data[0] << 8;
        angleLow = frame.data[1];
        angle = angleHigh | angleLow;

    }

    return angle;
}